#include "stdafx.h"
#include "Programs\Path_planner.h"
#include "Utils\PathUtils\HybridGraph.h"

#include "Utils\Rotation_utils\Rotation.h"
#include "Utils\ReferencePoint.h"

#include "Utils\UI_utils\Environment.h"
#include "Utils\UI_utils\TimeManager.h"
#include "Utils\UI_utils\Graphics.h"

#include "Utils\Number_utils\AK_conversions_1.h"
#include "Utils\Polygon_utils\PolygonIO.h"
#include "Path_planning\Motion_sequence.h"
#include "Mms_example.h"



typedef mms::Mms_path_planner_example<>             Planner;


void write_path(std::vector<Planner::Reference_point>const& path,string filename)
{
	std::ofstream pathFile;
	pathFile.open(filename);
	pathFile << path << endl;
	pathFile.close();
}

void display_scene(int argc, char* argv[])
{
  //typedefs
  //typedef mms::Mms_path_planner_example<>   Planner;

  //loading files from configuration file
  Time_manager tm;
  tm.write_time_log(std::string("start"));
  
  Environment<> env(argc,argv);
  tm.write_time_log(std::string("set environment"));

  //loading scene from environment
  Planner::Polygon_vec&       workspace(env.get_workspace());
  Planner::Extended_polygon   my_robot(env.get_robot());	

  //load query
  Planner::Reference_point q_s (env.get_source_configiration());
  Planner::Reference_point q_t (env.get_target_configiration());

  global_graphics->draw_polygons<Planner::K> (workspace, BLUE);
  my_robot.move_absolute(q_s);
  global_graphics->draw_polygon<Planner::K> (my_robot.get_absolute_polygon(), GREEN);
  my_robot.move_absolute(q_t);
  global_graphics->draw_polygon<Planner::K> (my_robot.get_absolute_polygon(), RED);
  

  global_graphics->display();
  return;
}


void single_robot_planner_example(int argc, char* argv[])
{
  //typedefs
  //typedef mms::Mms_path_planner_example<>             Planner;
  
  //loading files from configuration file
  Time_manager tm;
  tm.write_time_log(std::string("start"));
  
  Environment<> env(argc,argv);
  tm.write_time_log(std::string("set environment"));

  //loading scene from environment
  Planner::Polygon_vec&       workspace(env.get_workspace());
  Planner::Extended_polygon   my_robot(env.get_robot());	
  
  //initialize the planner and preprocess 
  Planner planner(workspace, my_robot);    
  planner.preprocess(); 
   
  //load query
  Planner::Reference_point q_s (env.get_source_configiration());
  Planner::Reference_point q_t (env.get_target_configiration());

  //perform query
  Motion_sequence<Planner::K> motion_sequence;
  bool found_path = planner.query(q_s, q_t, motion_sequence);

  if (!found_path)
    std::cout<<"no path found :-("<<std::endl;
  else
    std::cout<<"path found :-)"<<std::endl;

  //example of how to create a path 
  Motion_sequence<Planner::K>::MS_base_ptr ms_ptr;
  std::vector<Planner::Reference_point>				path;
  BOOST_FOREACH (ms_ptr, motion_sequence.get_sequence())
	  path.push_back(ms_ptr->source());
  path.push_back(q_t);

  write_path(path,"path.txt");
  
  global_graphics->draw_path(path, my_robot, true);

  global_graphics->draw_polygons<Planner::K> (workspace, BLUE);
  my_robot.move_absolute(q_s);
  global_graphics->draw_polygon<Planner::K> (my_robot.get_absolute_polygon(), GREEN);
  my_robot.move_absolute(q_t);
  global_graphics->draw_polygon<Planner::K> (my_robot.get_absolute_polygon(), RED);

  global_graphics->display();

  return;
}


void single_robot_planner_with_optimization(int argc, char* argv[], int num_iter=1)
{
  //typedefs
  //typedef mms::Mms_path_planner_example<>             Planner;
	typedef std::vector<Planner::Reference_point>		Path;
	typedef std::vector<Path>							Path_vector;

  //loading files from configuration file
  Time_manager tm;
  tm.write_time_log(std::string("start"));
  
  Environment<> env(argc,argv);
  tm.write_time_log(std::string("set environment"));

  //loading scene from environment
  Planner::Polygon_vec&       workspace(env.get_workspace());
  Planner::Extended_polygon   my_robot(env.get_robot());
   
  //load query
  Planner::Reference_point q_s (env.get_source_configiration());
  Planner::Reference_point q_t (env.get_target_configiration());

  Path_vector path_vec = Path_vector();

  H_Graph<Planner::K> hgraph(workspace,my_robot.get_original_polygon(),q_s,q_t);
  for (int t=0; t < num_iter; t++){

  //initialize the planner and preprocess 
  Planner planner(workspace, my_robot);    
  planner.preprocess(); 

  //perform query
  Motion_sequence<Planner::K> motion_sequence;
  bool found_path = planner.query(q_s, q_t, motion_sequence);

  //example of how to create a path 
  Motion_sequence<Planner::K>::MS_base_ptr ms_ptr;
  std::vector<Planner::Reference_point>				path;
  BOOST_FOREACH (ms_ptr, motion_sequence.get_sequence())
	  path.push_back(ms_ptr->source());
  path.push_back(q_t);
  std::stringstream filename;
  filename << "path" << t << ".txt";
  write_path(path,filename.str());
  path_vec.push_back(path);
  global_graphics->draw_path<Planner::K>(path);
  hgraph.add_path(path);
  }

  std::vector<Planner::Reference_point>	opt_path = hgraph.compute_optimal_path();
  
  global_graphics->draw_path(opt_path, my_robot, true);
  global_graphics->draw_polygons<Planner::K> (workspace, BLUE);
  my_robot.move_absolute(q_s);
  global_graphics->draw_polygon<Planner::K> (my_robot.get_absolute_polygon(), GREEN);
  my_robot.move_absolute(q_t);
  global_graphics->draw_polygon<Planner::K> (my_robot.get_absolute_polygon(), RED);

  global_graphics->display();

  return;
}



